#include "MPU9250.h"

#ifdef __cplusplus
extern "C" {
#endif

#define BIT_AUX_IF_EN       (0x20)
#define I2C_master
//#define debug_MPU9250WriteSecondary    //OK
//#define debug_MPU9250GyroRead 
//#define debug_MPU9250AccelRead
#define debug_MPU9250AccelRead_INT_STATUS
#define debug_MPU9250MagRead


/******************************************************************************
 * interface driver                                                           *
 ******************************************************************************/
#define IIC_Dev  "/dev/i2c-1"     //树莓派 & 友善 nano pi M4B
//#define IIC_Dev  "/dev/i2c-0"
int fd,fdmag;

Mpu_Data mpu_data_s;

char I2C_ReadOneByte(char reg)
{
	return wiringPiI2CReadReg8(fd, reg);
}

void I2C_WriteOneByte(char reg, char val)
{
	wiringPiI2CWriteReg8(fd, reg, val);
}

void MPU9250ReadSecondary(unsigned char  u8I2CAddr, unsigned char  u8RegAddr, unsigned char  u8Len, unsigned char  *pu8data)
{
    unsigned char  i;
    unsigned char  u8Temp;

    I2C_WriteOneByte(REG_I2C_SLV0_ADDR,u8I2CAddr);					//MAG address 0x8C (0x18不可用），and Transfer is a read
    I2C_WriteOneByte(REG_I2C_SLV0_REG,u8RegAddr);
	I2C_WriteOneByte(REG_I2C_SLV0_CTRL,REG_BIT_I2C_SLV0_EN|u8Len);	// 0x81 Enable reading data from this slave at the sample rate && 1 Byte length
/**********************************************************************************************************************************************************	
	I2C_WriteOneByte(REG_USER_CTRL,0x20);							//I2C master   default 00
	delay(100);
	res=I2C_ReadOneByte(REG_EXT_SENS_DATA_00);					//读取AK8975的ID
	printf("读取AK8975的ID: %x\r\n",res);
***********************************************************************************************************************************************************/    
    u8Temp = I2C_ReadOneByte(REG_USER_CTRL);
    u8Temp |= REG_BIT_I2C_MST_EN;
//	printf("u8Temp:%x\r\n",u8Temp);
    I2C_WriteOneByte(REG_USER_CTRL, u8Temp);
    delay(100);							//最低有效延迟
/**********************************************************************************************************************************************************
    u8Temp &= ~REG_BIT_I2C_MST_EN;
//	printf("u8Temp1:%x\r\n",u8Temp);
    I2C_WriteOneByte( REG_USER_CTRL, u8Temp);
	delay(20);
***********************************************************************************************************************************************************/    
    for(i=0; i<u8Len; i++)
    {
        *(pu8data+i) = I2C_ReadOneByte( REG_EXT_SENS_DATA_00+i);
        
    }
   
    u8Temp = I2C_ReadOneByte(REG_I2C_SLV0_CTRL);
    u8Temp &= ~((REG_BIT_I2C_MST_EN)&(I2C_SLV0_LENG_7));
    I2C_WriteOneByte( REG_I2C_SLV0_CTRL,  u8Temp);    

}

/*****************************ok***********************************/
void MPU9250WriteSecondary(unsigned char  u8I2CAddr, unsigned char  u8RegAddr, unsigned char  u8data)
{
  	unsigned char  u8Temp;			//OK

  	I2C_WriteOneByte( REG_I2C_SLV1_ADDR, u8I2CAddr);
  	I2C_WriteOneByte( REG_I2C_SLV1_REG,  u8RegAddr);
  	I2C_WriteOneByte( REG_I2C_SLV1_DO,   u8data);
  	I2C_WriteOneByte( REG_I2C_SLV1_CTRL, REG_BIT_I2C_SLV1_EN|1);

  	u8Temp = I2C_ReadOneByte(REG_USER_CTRL);
  	u8Temp |= REG_BIT_I2C_MST_EN;
#ifdef debug_MPU9250WriteSecondary
  	printf("REG_BIT_I2C_MST_EN : %x \r\n",u8Temp);
#endif
  	I2C_WriteOneByte( REG_USER_CTRL, u8Temp);
  	delay(100);
#ifdef debug_MPU9250WriteSecondary
  	u8Temp = I2C_ReadOneByte(REG_USER_CTRL);
  	printf("REG_BIT_I2C_MST_EN : %x \r\n",u8Temp);
#endif
/*****************************************************************
  	u8Temp &= ~REG_BIT_I2C_MST_EN;
  	I2C_WriteOneByte( REG_USER_CTRL, u8Temp);
******************************************************************/
	u8Temp = 0;
  	u8Temp = I2C_ReadOneByte(REG_I2C_SLV0_CTRL);
#ifdef debug_MPU9250WriteSecondary
	printf("REG_I2C_SLV0_CTRL : %x \r\n",u8Temp);
#endif
 	u8Temp &= ~((REG_BIT_I2C_SLV0_EN)&(I2C_SLV0_LENG_7));
#ifdef debug_MPU9250WriteSecondary
//	 u8Temp = 0x89;	//OK
	 printf("REG_BIT_I2C_SLV0_EN & I2C_SLV0_LENG_7 : %x \r\n",u8Temp);
#endif
  	I2C_WriteOneByte( REG_I2C_SLV0_CTRL,  u8Temp);
#ifdef debug_MPU9250WriteSecondary
	u8Temp = 0;
	u8Temp = I2C_ReadOneByte(REG_I2C_SLV0_CTRL);
	printf("REG_I2C_SLV0_CTRL3 : %x \r\n",u8Temp);  //OK
#endif

  
  return;
}

/*****************************ok***********************************/
bool MPU9250check(void)     	//OK
{
	bool bRet = false;
	short int ID;
	ID = I2C_ReadOneByte(REG_WHO_AM_I);
	printf("读取MPU9250的ID: %x\r\n",ID);
	if(MPU9250_ID == ID)
    {
        bRet = true;
    }
    return bRet;
}

/*****************************ok***********************************/
bool MPU9250MagCheck(void) //OK
{
    bool bRet = false;
    unsigned char  u8Ret[2];
	

    MPU9250ReadSecondary(I2C_MPU9250_AK8963 | I2C_MPU9250_AK8963_READ,REG_MAG_WIA1,2,u8Ret);

	printf("读取 AK8963 的ID: %x\r\n",u8Ret[0]);
	printf("读取 AK8963 的ID: %x\r\n",u8Ret[1]);

    if( (u8Ret[0] == REG_VAL_MAG_WIA1) && ( u8Ret[1] == REG_VAL_MAG_WIA2) )
    {
        bRet = true;
		printf("读取 AK8963 的ID: %x\r\n",u8Ret[0]);
		printf("读取 AK8963 的ID: %x\r\n",u8Ret[1]);
    }
    else
    {
		printf("读取 AK8963NN 的ID: %x\r\n",u8Ret[0]);
		printf("读取 AK8963NN 的ID: %x\r\n",u8Ret[1]);
	}
    return bRet;
}

/*****************************ok***********************************/
//设置MPU9250的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
unsigned int MPU_Set_LPF(unsigned int lpf)
{
	unsigned short int data;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6;
	I2C_WriteOneByte(REG_CONFIG,data);	  //设置数字低通滤波器
	return data=I2C_ReadOneByte(REG_CONFIG);
}

/*****************************ok***********************************/
//设置MPU9250的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
unsigned int MPU_Set_Rate(unsigned int rate)
{
	unsigned short int data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	I2C_WriteOneByte(REG_SMPLRT_DIV,data); //设置采样率50Hz
	data=I2C_ReadOneByte(REG_SMPLRT_DIV);  //设置数字低通滤波器
	return MPU_Set_LPF(rate/2);							  //自动设置LPF为采样率的一半
}

void MPU9250CalAvgValue(unsigned char  *pIndex, short int *pAvgBuffer, short int InVal, int *pOutVal)
{ 
  	unsigned char  i;
  
  	*(pAvgBuffer + ((*pIndex) ++)) = InVal;
    *pIndex &= 0x07;
    
    *pOutVal = 0;						//输出端口
  	for(i = 0; i < 8; i ++) 
    {
    	*pOutVal += *(pAvgBuffer + i);	//加8次
    }
    *pOutVal >>= 3; 					//除以8
}

void MPU9250GyroOffset(void)
{
  	unsigned char  i;
//  	short int s16Gx = 0, s16Gy = 0, s16Gz = 0;
  	int s32TempGx = 0, s32TempGy = 0, s32TempGz = 0;
  	for(i = 0; i < 32; i ++)
  	{
    	MPU9250GyroRead(&mpu_data_s);
/**********************************************
    	s32TempGx += s16Gx;
    	s32TempGy += s16Gy;
    	s32TempGz += s16Gz;
**********************************************/
		s32TempGx += mpu_data_s.GYRO.x;
		s32TempGy += mpu_data_s.GYRO.y;
    	s32TempGz += mpu_data_s.GYRO.z;
    	delay(10);
  	}
/*****************************************************
  	gstGyroOffset.s16X = s32TempGx >> 5;
  	gstGyroOffset.s16Y = s32TempGy >> 5;
  	gstGyroOffset.s16Z = s32TempGz >> 5;
*****************************************************/
	mpu_data_s.GyroOffset.x = s32TempGx >> 5;
	mpu_data_s.GyroOffset.y = s32TempGy >> 5;
	mpu_data_s.GyroOffset.z = s32TempGz >> 5;
	
  	return;
}


/******************************************************************************
 * icm20948 sensor device       OKOKOK                                                *
 ******************************************************************************/
void mpu9250init(void)
{
	short int res;

#ifndef I2C_master	
//可以使用
	I2C_WriteOneByte(REG_PWR_MGMT_1,REG_BIT_H_RESET);	//复位MPU9250
	delay(100);
	I2C_WriteOneByte(REG_PWR_MGMT_1,0x00);	//唤醒MPU9250
	I2C_WriteOneByte(REG_GYRO_CONFIG,REG_BIT_GYRO_FS_SEL_3);  //陀螺仪传感器,±2000dps
	I2C_WriteOneByte(REG_ACCEL_CONFIG,REG_BIT_ACCEL_FS_SEL_0);  //加速度传感器,±2g
	MPU_Set_Rate(50);						//设置采样率50Hz
	I2C_WriteOneByte(REG_INT_ENABLE,0x00);	//关闭所有中断	
	I2C_WriteOneByte(REG_USER_CTRL,0x00);	//I2C主模式关闭 0x00
	I2C_WriteOneByte(REG_FIFO_EN,0x00);	//关闭FIFO
	I2C_WriteOneByte(REG_INT_PIN_CFG,REG_BIT_ACTL | REG_BIT_BYPASS_EN);	//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
	
	res=I2C_ReadOneByte(REG_WHO_AM_I);	//读取MPU9250的ID
	printf("读取MPU9250的ID: %x\r\n",res);
	if(res==MPU9250_ID)//器件ID正确
	{
		I2C_WriteOneByte(REG_PWR_MGMT_1,REG_BIT_CLKSEL_1);	//设置CLKSEL,PLL X轴为参考
		I2C_WriteOneByte(REG_PWR_MGMT_2,0x00);				//加速度与陀螺仪都工作
		MPU_Set_Rate(50);						//设置采样率50Hz
	}
	else 
		return;
/*************************************************************************************************************************
	res = ~BIT_AUX_IF_EN;
	printf("BIT_AUX_IF_EN: %x\r\n",res);
	I2C_WriteOneByte(REG_USER_CTRL,0x00);
	I2C_WriteOneByte(REG_FIFO_EN,0x00); //关闭FIFO
	I2C_WriteOneByte(REG_INT_PIN_CFG,REG_BIT_ACTL | REG_BIT_LATCH_INT_EN | REG_BIT_INT_ANYRD_2CLEAR | REG_BIT_BYPASS_EN);

	//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
**************************************************************************************************************************/
	fdmag=wiringPiI2CSetup(I2C_MPU9250_AK8963); 	//打开 AK8963
	if(fdmag<0)
 	{
  		printf("IIC初始化失败\r\n");
		return fdmag;
  	}
	printf("fdmag:%d\r\n",fdmag);
	res=wiringPiI2CReadReg8(fdmag,0x00);	//读取AK8963 ID  
	printf("读取AK8963 ID  : %x\r\n",res);
	if(res==AK8963_ID)
    {
        wiringPiI2CWriteReg8(fdmag,REG_MAG_CNTL1,0X11);		//设置AK8963为单次测量模式
    }
	else 
		return;
	
#else

	I2C_WriteOneByte(REG_PWR_MGMT_1,REG_BIT_H_RESET);			//复位MPU9250
	delay(100);
	I2C_WriteOneByte(REG_PWR_MGMT_1,0x00);						//唤醒MPU9250
	I2C_WriteOneByte(REG_GYRO_CONFIG,REG_BIT_GYRO_FS_SEL_3);  	//陀螺仪传感器,±2000dps
	I2C_WriteOneByte(REG_ACCEL_CONFIG,REG_BIT_ACCEL_FS_SEL_0);  //加速度传感器,±2g
	MPU_Set_Rate(50);											//设置采样率50Hz
//	I2C_WriteOneByte(REG_INT_ENABLE,0x00);						//关闭所有中断	
	I2C_WriteOneByte(REG_INT_ENABLE,REG_BIT_RAW_RDY_EN);		//REG_BIT_WOM_EN，REG_INT_ENABLE 采用 REG_BIT_RAW_RDY_EN 这个模式不用设置其他也能实现50Hz中断
	I2C_WriteOneByte(REG_USER_CTRL,0x20);						//I2C主模式打开   		// 0x00：关闭
	I2C_WriteOneByte(REG_FIFO_EN,0x00);							//关闭FIFO
	I2C_WriteOneByte(REG_INT_PIN_CFG,REG_BIT_ACTL);				//INT引脚低电平有效 REG_BIT_ACTL
	
	res=I2C_ReadOneByte(REG_WHO_AM_I);							//读取MPU9250的ID
	printf("读取MPU9250的ID: %x\r\n",res);
	
	if(res==MPU9250_ID)//器件ID正确
	{
		I2C_WriteOneByte(REG_PWR_MGMT_1,REG_BIT_CLKSEL_1);		//设置CLKSEL,PLL X轴为参考
		I2C_WriteOneByte(REG_PWR_MGMT_2,0x00);					//加速度与陀螺仪都工作
		MPU_Set_Rate(50);										//设置采样率50Hz
	}
	else 
		return;

/*****************************************************************	
	I2C_WriteOneByte(REG_I2C_MST_CTRL,REG_BIT_WAIT_FOR_ES); //不配置不影响任何中断
	res=I2C_ReadOneByte(REG_I2C_MST_CTRL);
	printf("REG_I2C_MST_CTRL: %x\r\n",res);	
******************************************************************/
/**************************************************************************************************
	I2C_WriteOneByte(REG_MOT_DETECT_CTRL,REG_BIT_ACCEL_INTEL_EN | REG_BIT_ACCEL_INTEL_MODE);
	res=I2C_ReadOneByte(REG_I2C_MST_CTRL);
	printf("REG_I2C_MST_CTRL: %x\r\n",res);
	I2C_WriteOneByte(REG_WOM_THR,0x00);		//运动检测也能实现50HZ的中断，REG_INT_ENABLE 采用REG_BIT_WOM_EN中断
	res=I2C_ReadOneByte(REG_WOM_THR);
	printf("REG_WOM_THR: %x\r\n",res);
***************************************************************************************************/
/******************以上都没有问题******************************************/
	
	/* offset */
  	MPU9250GyroOffset();
	printf("STEP MAG\r\n");
/******电子罗盘判断********************************************************/
	MPU9250MagCheck();

	MPU9250WriteSecondary(I2C_MPU9250_AK8963 | I2C_MPU9250_AK8963_WRITE,REG_MAG_CNTL1, REG_BIT_MODE2 | REG_BIT_MODE1);
	
#endif
	return;
	
}

/******************************************************************************
 * IMU module        OKOKOK                                                   *
 ******************************************************************************/
#define Kp 4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 1.0f    // integral gain governs rate of convergence of gyroscope biases

float angles[3];
float q0, q1, q2, q3; 

bool imuInit(IMU_EN_SENSOR_TYPE *penMotionSensorType)
{
  bool bRet = false;
  
  wiringPiSetup();
  
  fd=wiringPiI2CSetup(I2C_MPU9250);
  if(fd<0)
  {
  	printf("IIC初始化失败\r\n");
	return false;
  }
  printf("fd:%d\r\n",fd);
  bRet = MPU9250check();
  if( true == bRet)
  {
    *penMotionSensorType = IMU_EN_SENSOR_TYPE_MPU9250;
    mpu9250init();
  }
  else
  {
    *penMotionSensorType = IMU_EN_SENSOR_TYPE_NULL;
	printf("IMU_EN_SENSOR_TYPE_NULL\r\n");
	return false;
  }
  
  q0 = 1.0f;  
  q1 = 0.0f;
  q2 = 0.0f;
  q3 = 0.0f;

  return true;
}

/*****************************ok***********************************/
void MPU9250GyroRead(Mpu_Data *pst)				//OK
{
	unsigned char  u8Buf[6];
    short int s16Buf[3] = {0}; 
    unsigned char  i;
    int s32OutBuf[3] = {0};
    static ICM20948_ST_AVG_DATA sstAvgBuf[3];
    static short int ss16c = 0;
    ss16c++;

    u8Buf[0]=I2C_ReadOneByte(REG_GYRO_XOUT_L); 	// u8Buf[0]=I2C_ReadOneByte(REG_GYRO_XOUT_L); 	//REG_ADD_GYRO_XOUT_L
    u8Buf[1]=I2C_ReadOneByte(REG_GYRO_XOUT_H);
    s16Buf[0]=  (u8Buf[1]<<8)|u8Buf[0];
	
#ifdef debug_MPU9250GyroRead
	printf("\r\n");
	printf("REG_GYRO_XOUT_L:%x\n",u8Buf[0]);
	printf("REG_GYRO_XOUT_H:%x\n",u8Buf[1]);
	printf("REG_GYRO_XOUT:%2x\n",s16Buf[0]);
#endif

    u8Buf[0]=I2C_ReadOneByte(REG_GYRO_YOUT_L); 
    u8Buf[1]=I2C_ReadOneByte(REG_GYRO_YOUT_H);
    s16Buf[1]=  (u8Buf[1]<<8)|u8Buf[0];

#ifdef debug_MPU9250GyroRead
	printf("\r\n");
	printf("REG_GYRO_YOUT_L:%x\n",u8Buf[0]);
	printf("REG_GYRO_YOUT_H:%x\n",u8Buf[1]);
	printf("REG_GYRO_YOUT:%2x\n",s16Buf[1]);
#endif

    u8Buf[0]=I2C_ReadOneByte(REG_GYRO_ZOUT_L); 
    u8Buf[1]=I2C_ReadOneByte(REG_GYRO_ZOUT_H);
    s16Buf[2]=  (u8Buf[1]<<8)|u8Buf[0];
	
#ifdef debug_MPU9250GyroRead
	printf("\r\n");
	printf("REG_GYRO_ZOUT_L:%x\n",u8Buf[0]);
	printf("REG_GYRO_ZOUT_H:%x\n",u8Buf[1]);
	printf("REG_GYRO_ZOUT:%2x\n",s16Buf[2]);
#endif

    for(i = 0; i < 3; i ++) 
    {
        MPU9250CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
    }
/***************************************************
    *ps16X = s32OutBuf[0] - gstGyroOffset.s16X;
    *ps16Y = s32OutBuf[1] - gstGyroOffset.s16Y;
    *ps16Z = s32OutBuf[2] - gstGyroOffset.s16Z;
***************************************************/
	pst->GYRO.x = s32OutBuf[0] - pst->GyroOffset.x;
    pst->GYRO.y = s32OutBuf[1] - pst->GyroOffset.y;
    pst->GYRO.z = s32OutBuf[2] - pst->GyroOffset.z;
	
#ifdef debug_MPU9250GyroRead
	printf("\r\n");
	printf("REG_GYRO_XOUT:%x\n",pst->GYRO.x);
	printf("REG_GYRO_YOUT:%x\n",pst->GYRO.y);
	printf("REG_GYRO_ZOUT:%x\n",pst->GYRO.z);
	u8Buf[0]=I2C_ReadOneByte(REG_WHO_AM_I);
	printf("REG_WHO_AM_I:%x\n",u8Buf[0]);
	u8Buf[1]=I2C_ReadOneByte(REG_SMPLRT_DIV);
	printf("REG_SMPLRT_DIV:%x\n",u8Buf[1]);
#endif

    return;
}

/*****************************ok***********************************/
void MPU9250AccelRead(Mpu_Data *pst)				//OKOK
{
    unsigned char  u8Buf[2];
    short int s16Buf[3] = {0}; 
    unsigned char  i;
    int s32OutBuf[3] = {0};
    static ICM20948_ST_AVG_DATA sstAvgBuf[3];
	

    u8Buf[0]=I2C_ReadOneByte(REG_ACCEL_XOUT_L); 
    u8Buf[1]=I2C_ReadOneByte(REG_ACCEL_XOUT_H);//  REG_ADD_ACCEL_XOUT_H
    s16Buf[0]=  (u8Buf[1]<<8)|u8Buf[0];
	
#ifdef debug_MPU9250AccelRead
	printf("\r\n");
	printf("REG_ACCEL_XOUT_L:%x\n",u8Buf[0]);
	printf("REG_ACCEL_XOUT_H:%x\n",u8Buf[1]);
	printf("REG_ACCEL_XOUT:%x\n",s16Buf[0]);
#endif


    u8Buf[0]=I2C_ReadOneByte(REG_ACCEL_YOUT_L); 
    u8Buf[1]=I2C_ReadOneByte(REG_ACCEL_YOUT_H);
    s16Buf[1]=  (u8Buf[1]<<8)|u8Buf[0];

#ifdef debug_MPU9250AccelRead
	printf("\r\n");
	printf("REG_ACCEL_YOUT_L:%x\n",u8Buf[0]);
	printf("REG_ACCEL_YOUT_H:%x\n",u8Buf[1]);
	printf("REG_ACCEL_YOUT:%x\n",s16Buf[1]);
#endif

    u8Buf[0]=I2C_ReadOneByte(REG_ACCEL_ZOUT_L); 
    u8Buf[1]=I2C_ReadOneByte(REG_ACCEL_ZOUT_H);
    s16Buf[2]=  (u8Buf[1]<<8)|u8Buf[0];

#ifdef debug_MPU9250AccelRead
	printf("\r\n");
	printf("REG_ACCEL_ZOUT_L:%x\n",u8Buf[0]);
	printf("REG_ACCEL_ZOUT_H:%x\n",u8Buf[1]);
	printf("REG_ACCEL_ZOUT:%x\n",s16Buf[2]);
#endif

    for(i = 0; i < 3; i ++) 
    {
        MPU9250CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
    }
/**************************************************
    *ps16X = s32OutBuf[0];
    *ps16Y = s32OutBuf[1];
    *ps16Z = s32OutBuf[2];
***************************************************/
	pst->ACC.x = s32OutBuf[0];
    pst->ACC.y = s32OutBuf[1];
    pst->ACC.z = s32OutBuf[2];

#ifdef debug_MPU9250AccelRead
	printf("\r\n");
	printf("REG_ACCEL_XOUT:%x\n",pst->ACC.x);
	printf("REG_ACCEL_YOUT:%x\n",pst->ACC.y);
	printf("REG_ACCEL_ZOUT:%x\n",pst->ACC.z);
	u8Buf[0]=I2C_ReadOneByte(REG_WHO_AM_I);
	printf("REG_WHO_AM_I:%x\n",u8Buf[0]);
	u8Buf[1]=I2C_ReadOneByte(REG_SMPLRT_DIV);
	printf("REG_SMPLRT_DIV:%x\n",u8Buf[1]);
#endif

	
	unsigned int Accel_INT_STATUS;
  	Accel_INT_STATUS=I2C_ReadOneByte(REG_INT_STATUS);
	
#ifdef debug_MPU9250AccelRead_INT_STATUS
	printf("\r\n");
	printf("INT_STATUS: %x\r\n",Accel_INT_STATUS);
#endif

	I2C_WriteOneByte(REG_INT_STATUS,0x00);
	Accel_INT_STATUS=I2C_ReadOneByte(REG_INT_STATUS);
	
#ifdef debug_MPU9250AccelRead_INT_STATUS
	printf("INT_STATUS_0: %x\r\n",Accel_INT_STATUS);
#endif	

    return;

}

void MPU9250MagRead(Mpu_Data *pst)
{
    unsigned char  counter = 20;
    unsigned char  u8Data[MAG_DATA_LEN];
	
#ifdef debug_MPU9250MagRead
	unsigned char  u8b[MAG_DATA_LEN];
#endif

    short int s16Buf[3] = {0,0,0}; 
    unsigned char  i;
    int s32OutBuf[3] = {0,0,0};
    static ICM20948_ST_AVG_DATA sstAvgBuf[3];
    while( counter>0 )
    {
        delay(10);
        MPU9250ReadSecondary( I2C_MPU9250_AK8963 | I2C_MPU9250_AK8963_READ,REG_MAG_ST1, 1, u8Data);
#ifdef debug_MPU9250MagRead
		MPU9250ReadSecondary( I2C_MPU9250_AK8963 | I2C_MPU9250_AK8963_READ,REG_MAG_WIA1,2, u8b);
		printf("\r\n");
		printf("REG_MAG_ST1:%x\n",u8Data[0]);
		printf("REG_MAG_ID1:%x\n",u8b[0]);
		printf("REG_MAG_ID2:%x\n",u8b[1]);
		printf("REG_MAG_ID3:%x\n",u8b[2]);
#endif
        if ((u8Data[0] & REG_BIT_DRDY) != 0)  //0x01
            break;
        
        counter--;
    }
    
    if(counter != 0)
    {
        MPU9250ReadSecondary( I2C_MPU9250_AK8963|I2C_MPU9250_AK8963_READ, REG_MAG_DATA_HXL, MAG_DATA_LEN,u8Data);
		
        s16Buf[0] = ((short int)u8Data[1]<<8) | u8Data[0];
        s16Buf[1] = ((short int)u8Data[3]<<8) | u8Data[2];
        s16Buf[2] = ((short int)u8Data[5]<<8) | u8Data[4];
		
#ifdef debug_MPU9250MagRead
		printf("\r\n");
		printf("REG_MAG_DATA_HXL:%x\n",u8Data[0]);
		printf("REG_MAG_DATA_HXH:%x\n",u8Data[1]);
		printf("REG_MAG_DATA_HYL:%x\n",u8Data[2]);
		printf("REG_MAG_DATA_HYH:%x\n",u8Data[3]);
		printf("REG_MAG_DATA_HZL:%x\n",u8Data[4]);
		printf("REG_MAG_DATA_HZH:%x\n",u8Data[5]);
		printf("\r\n");
		printf("REG_MAG_DATA_HxH:%x\n",s16Buf[0]);
		printf("REG_MAG_DATA_HyH:%x\n",s16Buf[1]);
		printf("REG_MAG_DATA_HzH:%x\n",s16Buf[2]);
#endif
    }

    for(i = 0; i < 3; i ++) 
    {
        MPU9250CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
    }
/**************************************************    
    *ps16X =  s32OutBuf[0];
    *ps16Y = -s32OutBuf[1];
    *ps16Z = -s32OutBuf[2];
***************************************************/
	pst->MAG.x =  s32OutBuf[0];
    pst->MAG.y = -s32OutBuf[1];
    pst->MAG.z = -s32OutBuf[2];

#ifdef debug_MPU9250MagRead
	printf("\r\n");
	printf("REG_Mag_XOUT:%x\n",pst->MAG.x);
	printf("REG_Mag_YOUT:%x\n",pst->MAG.y);
	printf("REG_Mag_ZOUT:%x\n",pst->MAG.z);
	u8Data[0]=I2C_ReadOneByte(REG_WHO_AM_I);
	printf("REG_WHO_AM_I:%x\n",u8Data[0]);
	u8Data[1]=I2C_ReadOneByte(REG_SMPLRT_DIV);
	printf("REG_SMPLRT_DIV:%x\n",u8Data[1]);
#endif

    return;
}

float invSqrt(float x) 
{
  float halfx = 0.5f * x;
  float y = x;
  
  long i = *(long*)&y;                //get bits for floating value
  i = 0x5f3759df - (i >> 1);          //gives initial guss you
  y = *(float*)&i;                    //convert bits back to float
  y = y * (1.5f - (halfx * y * y));   //newtop step, repeating increases accuracy
  
  return y;
}

void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
{
  float norm;
  float hx, hy, hz, bx, bz;
  float vx, vy, vz, wx, wy, wz;
  float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
  float ex, ey, ez, halfT = 0.024f;

  float q0q0 = q0 * q0;
  float q0q1 = q0 * q1;
  float q0q2 = q0 * q2;
  float q0q3 = q0 * q3;
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q2q2 = q2 * q2;   
  float q2q3 = q2 * q3;
  float q3q3 = q3 * q3;          

  norm = invSqrt(ax * ax + ay * ay + az * az);       
  ax = ax * norm;
  ay = ay * norm;
  az = az * norm;

  norm = invSqrt(mx * mx + my * my + mz * mz);          
  mx = mx * norm;
  my = my * norm;
  mz = mz * norm;

  // compute reference direction of flux
  hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
  hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
  hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);         
  bx = sqrt((hx * hx) + (hy * hy));
  bz = hz;     

  // estimated direction of gravity and flux (v and w)
  vx = 2 * (q1q3 - q0q2);
  vy = 2 * (q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3;
  wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
  wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
  wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);  

  // error is sum of cross product between reference direction of fields and direction measured by sensors
  ex = (ay * vz - az * vy) + (my * wz - mz * wy);
  ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
  ez = (ax * vy - ay * vx) + (mx * wy - my * wx);

  if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
  {
    exInt = exInt + ex * Ki * halfT;
    eyInt = eyInt + ey * Ki * halfT;  
    ezInt = ezInt + ez * Ki * halfT;

    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;
  }

  q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
  q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
  q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
  q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;  

  norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 = q0 * norm;
  q1 = q1 * norm;
  q2 = q2 * norm;
  q3 = q3 * norm;
}

void imuDataGet(Mpu_Data *pst)
{
	float MotionVal[9];
//	short int s16Gyro[3], s16Accel[3], s16Magn[3];
	
	MPU9250AccelRead(&mpu_data_s);
	MPU9250GyroRead(&mpu_data_s);
	MPU9250MagRead(&mpu_data_s);
	
	MotionVal[0] = pst->GYRO.x/32.8;
	MotionVal[1] = pst->GYRO.y/32.8;
	MotionVal[2] = pst->GYRO.z/32.8;
	MotionVal[3] = pst->ACC.x;
	MotionVal[4] = pst->ACC.y;
	MotionVal[5] = pst->ACC.z;
	MotionVal[6] = pst->MAG.x;
	MotionVal[7] = pst->MAG.y;
	MotionVal[8] = pst->MAG.z;
	imuAHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
					(float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], 
					(float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
/***********************************************************************************************************
	pstAngles->fPitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
	pstAngles->fRoll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
	pstAngles->fYaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3; 
	
	pstGyroRawData->s16X = s16Gyro[0];
	pstGyroRawData->s16Y = s16Gyro[1];
	pstGyroRawData->s16Z = s16Gyro[2];
	
	pstAccelRawData->s16X = s16Accel[0];
	pstAccelRawData->s16Y = s16Accel[1];
	pstAccelRawData->s16Z  = s16Accel[2];
	
	pstMagnRawData->s16X = s16Magn[0];
	pstMagnRawData->s16Y = s16Magn[1];
	pstMagnRawData->s16Z = s16Magn[2];  
***********************************************************************************************************/

	pst->Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
	pst->Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
	pst->Yaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;
	
/**********************************************************************************************************
	pst->GYRO.x = s16Gyro[0];
	pst->GYRO.y = s16Gyro[1];short int s16Gyro[3], s16Accel[3], s16Magn[3];
	pst->GYRO.z = s16Gyro[2];

	pst->ACC.x = s16Accel[0];
	pst->ACC.y = s16Accel[1];
	pst->ACC.z = s16Accel[2];

	pst->MAG.x = s16Magn[0];
	pst->MAG.y = s16Magn[1];
	pst->MAG.z = s16Magn[2];
***********************************************************************************************************/
	pst->GYRO.x = pst->GYRO.x;
	pst->GYRO.y = pst->GYRO.y;
	pst->GYRO.z = pst->GYRO.z;

	pst->ACC.x = pst->ACC.x;
	pst->ACC.y = pst->ACC.y;
	pst->ACC.z = pst->ACC.z;

	pst->MAG.x = pst->MAG.x;
	pst->MAG.y = pst->MAG.y;
	pst->MAG.z = pst->MAG.z;
	
	return;  
}


#ifdef __cplusplus
}
#endif
